#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");

/*
这里是一个简单的接收四元数，然后plan和excute
然后发送给
*/

class EegoalNode : public rclcpp::Node
{
public:
    EegoalNode(std::string name) : Node(name)
    {
        auto qos = rclcpp::QoS(1).reliable().transient_local();

        pub_goal = this->create_publisher<geometry_msgs::msg::Pose>(
            "ee_goal_pose",
            qos);
        // timer_ = this->create_wall_timer(
        //     std::chrono::seconds(1),
        //     std::bind(&EegoalNode::Send_goal_pose, this));

        geometry_msgs::msg::Pose msg;
        msg.orientation.w = 1.0;
        msg.position.x = 0.18;
        msg.position.y = -0.1;
        msg.position.z = 0.2;
        timer_ = this->create_wall_timer(std::chrono::milliseconds(500), [this]()
                                         {
            geometry_msgs::msg::Pose msg;
            msg.orientation.w = 0.8;
            msg.position.x = 0.15;
            msg.position.y = 0.14;
            msg.position.z = 0.1;
            pub_goal->publish(msg);
            RCLCPP_INFO(this->get_logger(), "Published delayed goal pose");
            // 取消定时器，只发布一次
            timer_->cancel(); });
        // pub_goal->publish(msg);
        RCLCPP_INFO(this->get_logger(), "Published test pose");
    };

private:
    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr pub_goal;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<EegoalNode>("test1_pose_node");

    // Initialize MoveGroupInterface after the node is fully constructed

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}